package gps.test;

import java.io.IOException;
import java.util.List;

import android.app.Activity;
import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.Canvas;
import android.graphics.Paint;
import android.hardware.Camera;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.hardware.Camera.Size;
import android.os.Bundle;
import android.view.Display;
import android.view.SurfaceHolder;
import android.view.SurfaceView;
import android.view.View;
import android.view.ViewGroup;
import android.view.Window;
import android.view.WindowManager;
import android.widget.RelativeLayout;
import android.widget.TextView;

public class AccelerometerTestActivity extends Activity implements SensorEventListener {
	private SensorManager mSensorManager;
	private Sensor mAccelerometer, mField;
	private TextView valueView, directionView;
	private DrawView mDrawView;
	private float[] mGravity;
	private float[] mMagnetic;
	//
	private CameraPreview  cPreview;

	public void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		//
		requestWindowFeature(Window.FEATURE_NO_TITLE);
		getWindow().setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN, WindowManager.LayoutParams.FLAG_FULLSCREEN);
		
		cPreview = new CameraPreview (this);
		valueView = new TextView(this);
		directionView = new TextView(this);
		mDrawView = new DrawView(this);

		RelativeLayout l = new RelativeLayout(this);
		l.addView(directionView, 0);
		l.addView(valueView);
		l.addView(cPreview, 0);
		l.addView(mDrawView);
		setContentView(l);

		mSensorManager = (SensorManager)getSystemService(SENSOR_SERVICE);
		mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		mField = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);

		//
		Display display = getWindowManager().getDefaultDisplay();
		ViewGroup.LayoutParams lParams = cPreview.getLayoutParams();
		lParams.height= display.getHeight();
		lParams.width = 4*lParams.height/3;
		cPreview.setLayoutParams(lParams);
	}
	
	@Override
	protected void onResume() {
		super.onResume();
		//
		cPreview.invalidate();
		mSensorManager.registerListener(this, mAccelerometer,
				SensorManager.SENSOR_DELAY_UI);
		mSensorManager.registerListener(this, mField, SensorManager.SENSOR_DELAY_UI);
	}
	
	@Override
	protected void onPause() {
		super.onPause();
		mSensorManager.unregisterListener(this);
	}
	private void updateDirection() {
		float[] temp = new float[9];
		float[] R = new float[9];
		//Load rotation matrix into R
		SensorManager.getRotationMatrix(temp, null, mGravity, mMagnetic);
		//Map to camera's point-of-view
		SensorManager.remapCoordinateSystem(temp, SensorManager.AXIS_X, SensorManager.AXIS_Z, R);
		//Return the orientation values
		float[] values = new float[3];
		SensorManager.getOrientation(R, values);
		//Convert to degrees
		for (int i=0; i < values.length; i++) {
			Double degrees = (values[i] * 180) / Math.PI;
			values[i] = degrees.floatValue();
		}
		//Display the compass direction
		directionView.setText( getDirectionFromDegrees(values[0]) );
		//Display the raw values
		valueView.setText(String.format("Azimuth: %1$1.2f, Pitch: %2$1.2f, Roll: %3$1.2f",
				values[0], values[1], values[2]));
		//----------------------------------------------------
		mDrawView.phoneAzimuth = (int)values[0];
		mDrawView.phonePitch = (int)values[1];
		mDrawView.phoneRoll = (int)values[2];
		mDrawView.invalidate();
	}
	private String getDirectionFromDegrees(float degrees) {
		if(degrees >= -22.5 && degrees < 22.5) { return "N"; }
		if(degrees >= 22.5 && degrees < 67.5) { return "NE"; }
		if(degrees >= 67.5 && degrees < 112.5) { return "E"; }
		if(degrees >= 112.5 && degrees < 157.5) { return "SE"; }
		if(degrees >= 157.5 || degrees < -157.5) { return "S"; }
		if(degrees >= -157.5 && degrees < -112.5) { return "SW"; }
		if(degrees >= -112.5 && degrees < -67.5) { return "W"; }
		if(degrees >= -67.5 && degrees < -22.5) { return "NW"; }
		return null;
	}
	public void onAccuracyChanged(Sensor sensor, int accuracy) { }
	public void onSensorChanged(SensorEvent event) {
		switch(event.sensor.getType()) {
		case Sensor.TYPE_ACCELEROMETER:
			mGravity = event.values.clone();
			break;
		case Sensor.TYPE_MAGNETIC_FIELD:
			mMagnetic = event.values.clone();
			break;
		default:
			return;
		}
		if(mGravity != null && mMagnetic != null) {
			updateDirection();
		}
	}
}

class DrawView extends View{

	public DrawView(Context context) {
		super(context);
		// TODO Auto-generated constructor stub
	}

	private final int ROLLORIGIN = 90;
	public int phoneAzimuth = 0 , phonePitch = 0, phoneRoll = 0;
	private float spanAzimuth, spanPitch;
	private float objectAzimuth = 0, objectPitch = 0;
	private int canvasHeight, canvasWidth;

	private int QuadrantNumber(float y, float x)
	{
		if(x >= 0 && y >= 0) return 1;
		if(x <= 0 && y >= 0) return 2;
		if(x <= 0 && y <= 0) return 3;
		if(x >= 0 && y <= 0) return 4;
		return 0;
	}
	
	@Override
	protected void onDraw(Canvas canvas) {
		// TODO Auto-generated method stub
		super.onDraw(canvas);
		canvasHeight = canvas.getHeight();
		canvasWidth = canvasHeight*4/3;
		Paint p = new Paint();
		p.setARGB(255, 150, 255, 0);
		p.setTextSize(30);
		p.setAntiAlias(true);
		Bitmap b = BitmapFactory.decodeResource(getResources(), R.drawable.test2);
		Bitmap bi = BitmapFactory.decodeResource(getResources(), R.drawable.test3);

		float spanRoll = phoneRoll + ROLLORIGIN; 
		//spanRoll ranges [0; 360]
		spanPitch = (phonePitch - objectPitch)*12;
		spanAzimuth = (phoneAzimuth - objectAzimuth)*12;

		float radius = (float)Math.sqrt((float)Math.pow(spanAzimuth, 2) + (float)Math.pow(spanPitch, 2));
		
		double alpha = Math.toDegrees(Math.atan(Math.abs(spanAzimuth/spanPitch))); 
		double betha = alpha;

		int quanrant = QuadrantNumber(spanPitch, spanAzimuth);
		if(quanrant == 2 || quanrant == 4)
		{
			betha -= spanRoll;
		}
		else
		{
			betha += spanRoll;
		}
		
		if(spanPitch >= 0)
		{
			spanPitch = (float)Math.cos(Math.toRadians(betha))*radius;
		}
		else
		{
			spanPitch = -(float)Math.cos(Math.toRadians(betha))*radius;
		}
		if(spanAzimuth >= 0)
		{
			spanAzimuth = (float)Math.sin(Math.toRadians(betha))*radius;	
		}
		else
		{
			spanAzimuth = -(float)Math.sin(Math.toRadians(betha))*radius;
		}
		
		float positionX = canvasWidth/2 - spanAzimuth;
		float positionY = canvasHeight/2 - spanPitch;
		canvas.drawBitmap(bi, positionX, positionY, p);

		//canvas.drawBitmap(b, canvasWidth/2- 120, canvasHeight/2 - 120, p);
		//canvas.drawBitmap(b, canvasWidth/2, canvasHeight/2, p);
		canvas.drawBitmap(b, canvasWidth/2-100, canvasHeight/2 - 100, p);
		canvas.drawBitmap(b, canvasWidth/2-100, canvasHeight/2 + 100, p);
		canvas.drawBitmap(b, canvasWidth/2+100, canvasHeight/2 + 100, p);
		canvas.drawBitmap(b, canvasWidth/2+100, canvasHeight/2 - 100, p);
		//canvas.drawBitmap(b, 0, canvasHeight/2, p);
		canvas.drawText(String.valueOf(alpha), canvasWidth - 100, 50, p);
		canvas.drawText(String.valueOf(betha), canvasWidth - 100, 150, p);
		canvas.drawText(String.valueOf(Math.cos(Math.toRadians(betha))), canvasWidth - 100, 250, p);
		canvas.drawText(String.valueOf(-Math.sin(Math.toRadians(betha))), canvasWidth - 100, 350, p);
		canvas.drawText(String.valueOf(spanRoll), canvasWidth - 100, 450, p);

	}
}

class CameraPreview extends SurfaceView implements SurfaceHolder.Callback {
	SurfaceHolder mHolder;
	Camera mCamera;

	CameraPreview(Context context) {
		super(context);

		mHolder = getHolder();
		mHolder.addCallback(this);
		mHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
	}


	public void surfaceCreated(SurfaceHolder holder) {
		// The Surface has been created, acquire the camera and tell it where
		// to draw.
		mCamera = Camera.open();
		try {
			mCamera.setPreviewDisplay(holder);
		} catch (IOException exception) {
			mCamera.release();
			mCamera = null;
			// TODO: add more exception handling logic here
		}
	}

	public void surfaceDestroyed(SurfaceHolder holder) {
		mCamera.stopPreview();
		mCamera.release();
		mCamera = null;
	}


	private Size getOptimalPreviewSize(List<Size> sizes, int w, int h) {
		final double ASPECT_TOLERANCE = 0.05;
		double targetRatio = (double) w / h;
		if (sizes == null) return null;

		Size optimalSize = null;
		double minDiff = Double.MAX_VALUE;

		int targetHeight = h;

		// Try to find an size match aspect ratio and size
		for (Size size : sizes) {
			double ratio = (double) size.width / size.height;
			if (Math.abs(ratio - targetRatio) > ASPECT_TOLERANCE) continue;
			if (Math.abs(size.height - targetHeight) < minDiff) {
				optimalSize = size;
				minDiff = Math.abs(size.height - targetHeight);
			}
		}

		// Cannot find the one match the aspect ratio, ignore the requirement
		if (optimalSize == null) {
			minDiff = Double.MAX_VALUE;
			for (Size size : sizes) {
				if (Math.abs(size.height - targetHeight) < minDiff) {
					optimalSize = size;
					minDiff = Math.abs(size.height - targetHeight);
				}
			}
		}
		return optimalSize;
	}

	public void surfaceChanged(SurfaceHolder holder, int format, int w, int h) {
		// Now that the size is known, set up the camera parameters and begin
		// the preview.
		Camera.Parameters parameters = mCamera.getParameters();

		List<Size> sizes = parameters.getSupportedPreviewSizes();
		Size optimalSize = getOptimalPreviewSize(sizes, w, h);
		parameters.setPreviewSize(optimalSize.width, optimalSize.height);

		mCamera.setParameters(parameters);   
		mCamera.startPreview();
	}
}